//
//  TheoJansen.m
//  PezMonkey
//
//  Created by Spence DiNicolantonio on 12/24/09.
//  Copyright 2009 Necrosoft. All rights reserved.
//

#import "TheoJansen.h"

#define kTheoMotorRate 3
#define kTheoMotorCoefFast 3
#define kTheoMotorCoefNormal 2
#define kTheoMotorCoefSlow 1

#define kTheoLowerLegGroup 2
#define kTheoUpperLegGroup 3
#define kTheoChassisGroup 4

#define kTheoSegWidth 6.0

#define kTheoLegCountPerSide 2

@implementation TheoJansen

@synthesize staticBody;
@synthesize motor;

- (NSString *)demoName {
	return @"Theo Jansen's Animal";
}

- (void)start {
	// create space
	self.space = [PMSpace spaceWithGravity:PMVectMake(0, -1)];
	[space setSubSteps:2];
	[space setIterations:13];
	
	// reset shape id counter
	[PMShape resetShapeIDCounter];
	
	// create shared static body
	self.staticBody = [PMBody staticBody];
	
	// create screen boundary
	[self addScreenBoundary];
	
	
	PMShape *shape;
	PMConstraint *joint;
	PMVect start, end;
	
	
	// define chassis attributes
	PMFloat chassisMass = 2.0;
	PMFloat chassisWidth = kTheoSegWidth;
	PMFloat chassisLength = 30.0;
	start = PMVectMake(-chassisLength, 0.0);
	end = PMVectMake(chassisLength, 0.0);
	PMFloat chassisMoment = PMMomentForSegment(chassisMass, start, end);
	
	// create chassis body
	PMBody *chassis = [PMBody bodyWithMass:chassisMass 
									moment:chassisMoment];
	[space addBody:chassis];
	
	// create chasses shape
	shape = [PMSegmentShape segmentShapeWithBody:chassis 
										   start:start 
											 end:end
										   width:chassisWidth];
	[shape setCollisionGroup:kTheoChassisGroup];
	[space addShape:shape];

	
	// define crank attributes
	PMFloat crankMass = 1.0;
	PMFloat crankRadius = 13.0;
	PMFloat crankMoment = PMMomentForCircle(crankMass, 0, crankRadius, PMVectZero);
	
	// crete crank body
	PMBody *crank = [PMBody bodyWithMass:crankMass 
								  moment:crankMoment];
	[space addBody:crank];
	
	// create crank shape
	shape = [PMCircleShape circleShapeWithBody:crank 
										radius:crankRadius];
	[shape setCollisionGroup:kTheoChassisGroup];
	[space addShape:shape];
	
	
	// attach crank to chassis
	joint = [PMPivotJoint pivotJointWithBodyA:chassis 
										bodyB:crank];
	[space addConstraint:joint];
	
	
	// create legs
	PMVect anchor;
	PMFloat side = 30.0f;
	PMFloat legCount = kTheoLegCountPerSide;
	for (int i=0; i<legCount; i++){
		
		// calculate anchor for right leg(s)
		anchor = PMVectForAngle((2.0*i) / legCount * M_PI);
		anchor = PMVectMult(anchor, crankRadius);
		
		// create left leg(s)
		[self AddLegWithChasis:chassis 
						 crank:crank 
						anchor:anchor
						onSide:side 
						offset:chassisLength];
		
		// calculate anchor for right leg(s)
		anchor = PMVectForAngle((2.0*i + 1) / legCount * M_PI);
		anchor = PMVectMult(anchor, crankRadius);
		
		// create left leg(s)
		[self AddLegWithChasis:chassis 
						 crank:crank 
						anchor:anchor
						onSide:side 
						offset:-chassisLength];
	}
	
	// attach motor to crank
	self.motor = [PMSimpleMotor simpleMotorWithBodyA:crank 
											   bodyB:chassis
												rate:6.0];
	[space addConstraint:motor];
}

- (void)update:(NSTimeInterval)deltaT {
	// determine motor max speed coefficient
	PMFloat coef = kTheoMotorCoefNormal;
	if (upArrow^downArrow)
		coef = (upArrow)? kTheoMotorCoefFast : kTheoMotorCoefSlow;
	
	// determine motor rate
	PMFloat rate = 0;
	if (leftArrow^rightArrow)
		rate = (leftArrow)? kTheoMotorRate : -kTheoMotorRate;
	rate *= coef;
	
	// set motor rate
	[motor setRate:rate];
	
	// update max force
	PMFloat max = (rate)? 170000.0 : 0.0;
	[motor setMaxForce:max];
	
}

/**
 * Creates a leg for the demo animal and adds it to the space.
 */
- (void)AddLegWithChasis:(PMBody *)chassis crank:(PMBody *)crank anchor:(PMAnchor)anchor onSide:(PMFloat)side offset:(PMFloat)offset {
	PMVect legStart;
	PMVect legEnd;
	PMShape *shape;
	PMConstraint *joint;
	
	// define shared leg attributes
	PMFloat legMass = 1.0;
	PMFloat legWidth = kTheoSegWidth;
	
	// define upper leg attributes
	legStart = PMVectZero;
	legEnd = PMVectMake(0.0, side);
	PMFloat upperLegMoment = PMMomentForSegment(legMass, legStart, legEnd);
	
	// create upper leg body
	PMBody *upperLeg = [PMBody bodyWithMass:legMass 
									moment:upperLegMoment];
	[upperLeg setPosition:PMVectMake(offset, 0)];
	[space addBody:upperLeg];
	
	// create upper leg shape
	shape = [PMSegmentShape segmentShapeWithBody:upperLeg 
										   start:legStart 
											 end:legEnd
										   width:legWidth];
	[shape setCollisionGroup:kTheoUpperLegGroup];
	[space addShape:shape];
	
	// attach upper leg to chassis
	joint = [PMPivotJoint pivotJointWithAnchor:PMVectMake(offset, 0.0) 
									   onBodyA:chassis 
										anchor:PMVectZero 
									   onBodyB:upperLeg];
	[space addConstraint:joint];
	
	
	
	// define lower leg attributes
	legStart = PMVectZero;
	legEnd = PMVectMake(0.0, -side);
	PMFloat lowerLegMoment = PMMomentForSegment(legMass, legStart, legEnd);
	
	// create lower leg body
	PMBody *lowerLeg = [PMBody bodyWithMass:legMass 
									 moment:lowerLegMoment];
	[lowerLeg setPosition:PMVectMake(offset, -side)];
	[space addBody:lowerLeg];
	
	// create lower leg shape
	shape = [PMSegmentShape segmentShapeWithBody:lowerLeg 
										   start:legStart 
											 end:legEnd
										   width:legWidth];
	[shape setCollisionGroup:kTheoLowerLegGroup];
	[space addShape:shape];
	shape = [PMCircleShape circleShapeWithBody:lowerLeg 
										radius:legWidth
										center:legEnd];
	[shape setFriction:0.5];
	[shape setElasticity:0.0];
	[shape setCollisionGroup:kTheoLowerLegGroup];
	[space addShape:shape];
	
	// attach lower leg to chassis
	joint = [PMPinJoint pinJointWithAnchor:PMVectMake(offset, 0.0) 
								   onBodyA:chassis 
									anchor:PMVectZero 
								   onBodyB:lowerLeg];
	[space addConstraint:joint];
	
	// attach gear joint to upper leg and lower leg
	joint = [PMGearJoint gearJointWithBodyA:upperLeg 
									  bodyB:lowerLeg 
									  phase:0 
									  ratio:1];
	[space addConstraint:joint];
	
	
	PMPinJoint *pinJoint;
	
	// define joint distance between leg segment and crank
	PMFloat diag = sqrtf(side*side + offset*offset);
	
	// attach upper leg to crank
	pinJoint = [PMPinJoint pinJointWithAnchor:anchor 
									  onBodyA:crank 
									   anchor:PMVectMake(0, side) 
									  onBodyB:upperLeg];
	[pinJoint setDistance:diag];
	[space addConstraint:pinJoint];
	
	// attach lower leg to crank
	pinJoint = [PMPinJoint pinJointWithAnchor:anchor 
									  onBodyA:crank 
									   anchor:PMVectZero
									  onBodyB:lowerLeg];
	[pinJoint setDistance:diag];
	[space addConstraint:pinJoint];
}

/**
 * Creates and segment shapes to demo space to make a boundary along the edges of
 * the screen.
 */
- (void)addScreenBoundary {
	// make sure static body has been initialized
	if (!staticBody)
		self.staticBody = [PMBody staticBody];
	
	// create temp shape
	PMShape *shape;
	
	// define boundary segment width 
	// (needs to be thick to avoid penetration at high velocities)
	PMFloat width = 30;
	
	PMFloat vertCount = 4;
	
	// create left screen boundary
	PMVect leftVerts[] = {
		PMVectMake(-320 - width, -240),
		PMVectMake(-320 - width, 240),
		PMVectMake(-320, 240),
		PMVectMake(-320, -240)
	};
	shape = [PMPolygonShape polygonShapeWithBody:staticBody 
										vertices:leftVerts 
									 vertCount:vertCount];
	[shape setElasticity:1.0];
	[shape setFriction:1.0];
	[shape setLayers:PMLayerMaskNotGrabbable];
	[space addStaticShape:shape];
	
	// create right screen boundary
	PMVect rightVerts[] = {
		PMVectMake(320 + width, 240),
		PMVectMake(320 + width, -240),
		PMVectMake(320, -240),
		PMVectMake(320, 240),
	};
	shape = [PMPolygonShape polygonShapeWithBody:staticBody 
										vertices:rightVerts 
									 vertCount:vertCount];
	[shape setElasticity:1.0];
	[shape setFriction:1.0];
	[shape setLayers:PMLayerMaskNotGrabbable];
	[space addStaticShape:shape];
	
	// create bottom screen boundary
	PMVect bottomVerts[] = {
		PMVectMake(-320, -240 - width),
		PMVectMake(-320, -240),
		PMVectMake(320, -240),
		PMVectMake(320, -240 - width),
	};
	shape = [PMPolygonShape polygonShapeWithBody:staticBody 
										vertices:bottomVerts 
									 vertCount:vertCount];
	[shape setElasticity:1.0];
	[shape setFriction:1.0];
	[shape setLayers:PMLayerMaskNotGrabbable];
	[space addStaticShape:shape];
}


#pragma mark -
#pragma mark Dealloc

- (void)dealloc {
	[staticBody release];
	[motor release];
	[super dealloc];
}

@end
